/**
  ******************************************************************************
  * File Name          : sensortsk.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "bsp.h"
#include "tskcfg.h"
#include "sensor.h"
#include "gv.h"
#include "measure.h"
#include <string.h>

TaskHandle_t xSensor_Thd = NULL;
StaticTask_t xSensor_Tcb;
StackType_t  xSensor_Stk[SENSOR_STACK_SIZE];

Sensor_BufStru sensor_rxbuf_bck;
Sensor_BufStru sensor_txbuf;

static Sensor_WadFilterStru filter_buf_stru[4];
static uint8_t wad_err_cnt[4];

static uint8_t sensor_get_wtcode_state(uint8_t index)
{
  uint8_t  addr;
  uint16_t crc;
  uint32_t wtcode;
  
  switch(index)
  {
    case 0:
      addr = gv.hr.meas_pa.wt1_sensor_addr;
      break;

    case 1:
      addr = gv.hr.meas_pa.wt2_sensor_addr;
      break;

    case 2:
      addr = gv.hr.meas_pa.wt3_sensor_addr;
      break;

    case 3:
      addr = gv.hr.meas_pa.wt4_sensor_addr;
      break;

    default:
      return 1;
  }

  if(addr == 0)
  {
    return 1;
  }

  sensor_txbuf.buf[0] = addr;
  sensor_txbuf.buf[1] = 0x04u;
  sensor_txbuf.buf[2] = 0x00u;
  sensor_txbuf.buf[3] = 0x04u;
  sensor_txbuf.buf[4] = 0x00u;
  sensor_txbuf.buf[5] = 0x03u;
  
  crc = sys_crc16(sensor_txbuf.buf, 6);
  
  sensor_txbuf.buf[6] = (uint8_t)crc;
  sensor_txbuf.buf[7] = (uint8_t)(crc >> 8);
  
  xQueueReset(sensor_recv_qhd);
  
  sensor_dma_send_buf(sensor_txbuf.buf, 8);
  
  if(xQueueReceive(sensor_recv_qhd, &sensor_rxbuf_bck, SENSOR_RECV_TIMEOUT) != pdTRUE)
  {
    return 3;
  }
  
  if(sensor_rxbuf_bck.cnt != 11)
  {
    return 4;
  }
  
  if(sensor_rxbuf_bck.buf[0] != addr)
  {
    return 5;
  }
  
  if((sensor_rxbuf_bck.buf[1] != 0x04) || (sensor_rxbuf_bck.buf[2] != 0x06))
  {
    return 6;
  }
  
  crc = (((uint16_t)sensor_rxbuf_bck.buf[10]) << 8) + sensor_rxbuf_bck.buf[9];
  if(crc != sys_crc16(sensor_rxbuf_bck.buf, 9))
  {
    return 7;
  }

  wtcode = (((uint32_t)sensor_rxbuf_bck.buf[3]) << 24) + (((uint32_t)sensor_rxbuf_bck.buf[4]) << 16) + \
           (((uint32_t)sensor_rxbuf_bck.buf[5]) << 8) + sensor_rxbuf_bck.buf[6];
  
  gv.ir.meas_ca.wt_sensor_info[index].srccode = (int32_t)wtcode;

  gv.ir.meas_ca.wt_sensor_info[index].state = (((uint16_t)sensor_rxbuf_bck.buf[7]) << 8) + sensor_rxbuf_bck.buf[8];
  
  return 0;
}

static uint8_t sensor_get_spdcode_state(void)
{
  uint8_t  addr;
  uint16_t crc;
  
  if(gv.hr.meas_pa.spd_sensor_addr == 0)
  {
    return 1;
  }

  addr = gv.hr.meas_pa.spd_sensor_addr;

  sensor_txbuf.buf[0] = addr;
  sensor_txbuf.buf[1] = 0x04u;
  sensor_txbuf.buf[2] = 0x00u;
  sensor_txbuf.buf[3] = 0x04u;
  sensor_txbuf.buf[4] = 0x00u;
  sensor_txbuf.buf[5] = 0x03u;
  
  crc = sys_crc16(sensor_txbuf.buf, 6);
  
  sensor_txbuf.buf[6] = (uint8_t)crc;
  sensor_txbuf.buf[7] = (uint8_t)(crc >> 8);
  
  xQueueReset(sensor_recv_qhd);
  
  sensor_dma_send_buf(sensor_txbuf.buf, 8);
  
  if(xQueueReceive(sensor_recv_qhd, &sensor_rxbuf_bck, SENSOR_RECV_TIMEOUT) != pdTRUE)
  {
    return 2;
  }
  
  if(sensor_rxbuf_bck.cnt != 11)
  {
    return 3;
  }
  
  if(sensor_rxbuf_bck.buf[0] != addr)
  {
    return 4;
  }
  
  if((sensor_rxbuf_bck.buf[1] != 0x04) || (sensor_rxbuf_bck.buf[2] != 0x06))
  {
    return 5;
  }
  
  crc = (((uint16_t)sensor_rxbuf_bck.buf[10]) << 8) + sensor_rxbuf_bck.buf[9];
  if(crc != sys_crc16(sensor_rxbuf_bck.buf, 9))
  {
    return 6;
  }
  
  gv.ir.meas_ca.spd_sensor_info.srccode = (((uint32_t)sensor_rxbuf_bck.buf[3]) << 8) + sensor_rxbuf_bck.buf[4];

  gv.ir.meas_ca.spd_sensor_info.state = (((uint16_t)sensor_rxbuf_bck.buf[7]) << 8) + sensor_rxbuf_bck.buf[8];

  return 0;
}

static uint8_t sensor_get_wtall(uint8_t index)
{
  uint8_t  addr;
  uint16_t crc;
  uint16_t temp;
  uint32_t wtcode;
  
  switch(index)
  {
    case 0:
      addr = gv.hr.meas_pa.wt1_sensor_addr;
      break;

    case 1:
      addr = gv.hr.meas_pa.wt2_sensor_addr;
      break;

    case 2:
      addr = gv.hr.meas_pa.wt3_sensor_addr;
      break;

    case 3:
      addr = gv.hr.meas_pa.wt4_sensor_addr;
      break;

    default:
      return 1;
  }

  if(addr == 0)
  {
    return 1;
  }

  sensor_txbuf.buf[0] = addr;
  sensor_txbuf.buf[1] = 0x04u;
  sensor_txbuf.buf[2] = 0x00u;
  sensor_txbuf.buf[3] = 0x00u;
  sensor_txbuf.buf[4] = 0x00u;
  sensor_txbuf.buf[5] = 0x0Bu;   /* 读取11个寄存器值 */
  
  crc = sys_crc16(sensor_txbuf.buf, 6);
  
  sensor_txbuf.buf[6] = (uint8_t)crc;
  sensor_txbuf.buf[7] = (uint8_t)(crc >> 8);
  
  xQueueReset(sensor_recv_qhd);
  
  sensor_dma_send_buf(sensor_txbuf.buf, 8);
  
  if(xQueueReceive(sensor_recv_qhd, &sensor_rxbuf_bck, SENSOR_RECV_TIMEOUT) != pdTRUE)
  {
    return 3;
  }
  
  if(sensor_rxbuf_bck.cnt != 27)
  {
    return 4;
  }
  
  if(sensor_rxbuf_bck.buf[0] != addr)
  {
    return 5;
  }
  
  if((sensor_rxbuf_bck.buf[1] != 0x04) || (sensor_rxbuf_bck.buf[2] != 22))
  {
    return 6;
  }
  
  crc = (((uint16_t)sensor_rxbuf_bck.buf[26]) << 8) + sensor_rxbuf_bck.buf[25];
  if(crc != sys_crc16(sensor_rxbuf_bck.buf, 25))
  {
    return 7;
  }

  gv.ir.meas_ca.wt_sensor_info[index].sn = (((uint64_t)sensor_rxbuf_bck.buf[3]) << 56) + (((uint64_t)sensor_rxbuf_bck.buf[4]) << 48) + \
                                           (((uint64_t)sensor_rxbuf_bck.buf[5]) << 40) + (((uint64_t)sensor_rxbuf_bck.buf[6]) << 32) + \
                                           (((uint64_t)sensor_rxbuf_bck.buf[7]) << 24) + (((uint64_t)sensor_rxbuf_bck.buf[8]) << 16) + \
                                           (((uint64_t)sensor_rxbuf_bck.buf[9]) << 8)  + sensor_rxbuf_bck.buf[10];

  wtcode = (((uint32_t)sensor_rxbuf_bck.buf[11]) << 24) + (((uint32_t)sensor_rxbuf_bck.buf[12]) << 16) + \
           (((uint32_t)sensor_rxbuf_bck.buf[13]) << 8) + sensor_rxbuf_bck.buf[14];
  gv.ir.meas_ca.wt_sensor_info[index].srccode = (int32_t)wtcode;

  gv.ir.meas_ca.wt_sensor_info[index].state = (((uint16_t)sensor_rxbuf_bck.buf[15]) << 8) + sensor_rxbuf_bck.buf[16];

  gv.ir.meas_ca.wt_sensor_info[index].range = (((uint32_t)sensor_rxbuf_bck.buf[17]) << 24) + (((uint32_t)sensor_rxbuf_bck.buf[18]) << 16) + \
                                              (((uint32_t)sensor_rxbuf_bck.buf[19]) << 8)  + sensor_rxbuf_bck.buf[20];
  
  temp = (((uint16_t)sensor_rxbuf_bck.buf[21]) << 8) + sensor_rxbuf_bck.buf[22];
  gv.ir.meas_ca.wt_sensor_info[index].temperature = (int16_t)temp;
 
  gv.ir.meas_ca.wt_sensor_info[index].sv = (((uint16_t)sensor_rxbuf_bck.buf[23]) << 8) + sensor_rxbuf_bck.buf[24];

  return 0;
}

static uint8_t sensor_get_spdall(void)
{
  uint8_t  addr;
  uint16_t crc;
  uint16_t temp;

  if(gv.hr.meas_pa.spd_sensor_addr == 0)
  {
    return 1;
  }

  addr = gv.hr.meas_pa.spd_sensor_addr;

  sensor_txbuf.buf[0] = addr;
  sensor_txbuf.buf[1] = 0x04u;
  sensor_txbuf.buf[2] = 0x00u;
  sensor_txbuf.buf[3] = 0x00u;
  sensor_txbuf.buf[4] = 0x00u;
  sensor_txbuf.buf[5] = 0x0Bu;   /* 读取11个寄存器值 */
  
  crc = sys_crc16(sensor_txbuf.buf, 6);
  
  sensor_txbuf.buf[6] = (uint8_t)crc;
  sensor_txbuf.buf[7] = (uint8_t)(crc >> 8);
  
  xQueueReset(sensor_recv_qhd);
  
  sensor_dma_send_buf(sensor_txbuf.buf, 8);
  
  if(xQueueReceive(sensor_recv_qhd, &sensor_rxbuf_bck, SENSOR_RECV_TIMEOUT) != pdTRUE)
  {
    return 2;
  }
  
  if(sensor_rxbuf_bck.cnt != 27)
  {
    return 3;
  }
  
  if(sensor_rxbuf_bck.buf[0] != addr)
  {
    return 4;
  }
  
  if((sensor_rxbuf_bck.buf[1] != 0x04) || (sensor_rxbuf_bck.buf[2] != 22))
  {
    return 5;
  }
  
  crc = (((uint16_t)sensor_rxbuf_bck.buf[26]) << 8) + sensor_rxbuf_bck.buf[25];
  if(crc != sys_crc16(sensor_rxbuf_bck.buf, 25))
  {
    return 6;
  }
  
  gv.ir.meas_ca.spd_sensor_info.sn = (((uint64_t)sensor_rxbuf_bck.buf[3]) << 56) + (((uint64_t)sensor_rxbuf_bck.buf[4]) << 48) + \
                                     (((uint64_t)sensor_rxbuf_bck.buf[5]) << 40) + (((uint64_t)sensor_rxbuf_bck.buf[6]) << 32) + \
                                     (((uint64_t)sensor_rxbuf_bck.buf[7]) << 24) + (((uint64_t)sensor_rxbuf_bck.buf[8]) << 16) + \
                                     (((uint64_t)sensor_rxbuf_bck.buf[9]) << 8)  + sensor_rxbuf_bck.buf[10];
                                     
  gv.ir.meas_ca.spd_sensor_info.srccode = (((uint16_t)sensor_rxbuf_bck.buf[11]) << 8) + sensor_rxbuf_bck.buf[12];

  /* 13 14 reserved */
  
  gv.ir.meas_ca.spd_sensor_info.state = (((uint16_t)sensor_rxbuf_bck.buf[15]) << 8) + sensor_rxbuf_bck.buf[16];

  gv.ir.meas_ca.spd_sensor_info.diameter = (((uint16_t)sensor_rxbuf_bck.buf[17]) << 8) + sensor_rxbuf_bck.buf[18];
  gv.ir.meas_ca.spd_sensor_info.gears = (((uint16_t)sensor_rxbuf_bck.buf[19]) << 8) + sensor_rxbuf_bck.buf[20];

  temp = (((uint16_t)sensor_rxbuf_bck.buf[21]) << 8) + sensor_rxbuf_bck.buf[22];
  gv.ir.meas_ca.spd_sensor_info.temperature = (int16_t)temp;
 
  gv.ir.meas_ca.spd_sensor_info.sv = (((uint16_t)sensor_rxbuf_bck.buf[23]) << 8) + sensor_rxbuf_bck.buf[24];

  return 0;
}

static void wad_filter_init(void)
{
  memset(filter_buf_stru, 0, sizeof(filter_buf_stru));  /* TODO: 打印一下大小看看 */
}

int32_t wad_filter(Sensor_WadFilterStru *ft, int32_t indata)
{
  int32_t i, ave;
  int32_t sum;
  
  ft->buf[ft->cnt] = indata;
  if(ft->cnt >= SENSOR_WAD_FILTER_WINDOW)
  {
    ft->cnt = 0;
  }
  
  sum = ft->buf[0];
  
  for(i=1; i<SENSOR_WAD_FILTER_WINDOW; i++)
  {
    sum += ft->buf[i];
  }
  
  ave = sum / SENSOR_WAD_FILTER_WINDOW;
  
  return ave;
}

void vSensor_Task(void * pvParameters)
{
  uint8_t i;
  uint8_t res;

  sensor_uart_init();

  wad_filter_init();

  wad_err_cnt[0] = 0;
  wad_err_cnt[1] = 0;
  wad_err_cnt[2] = 0;
  wad_err_cnt[3] = 0;

  while(1)
  {
    if(gv.ir.meas_tp.sensor_full_info != 0)
    {
      for(i=0; i<4; i++)
      {
        res = sensor_get_wtall(i);
        if(res == 1) /* addr == 0 直接跳过 */
        {
          gv.ir.meas_ad.wad_divided[i] = 0;
          continue;
        }
        else if((res == 0) && (gv.ir.meas_ca.wt_sensor_info[i].state == 0))
        {
          wad_err_cnt[i] = 0;
          gv.ir.meas_ad.wad_divided[i] = wad_filter(&filter_buf_stru[i], (gv.ir.meas_ca.wt_sensor_info[i].srccode >> 6));
        }
        else
        {
          /* TODO: gv.ir.meas_ad.wad_divided[i] 暂且维持原值,等更合理的逻辑出来后再处理 */
          if(wad_err_cnt[i] < SENSOR_WAD_ERRCNT_MAX){
            wad_err_cnt[i]++;
          }else{
            wad_err_cnt[i] = SENSOR_WAD_ERRCNT_MAX;
          }
        }

        vTaskDelay(1);
      }

      sensor_get_spdall();
      vTaskDelay(1);
    }
    else
    {
      for(i=0; i<4; i++)
      {
        res = sensor_get_wtcode_state(i);
        if(res == 1) /* addr == 0 直接跳过 */
        {
          gv.ir.meas_ad.wad_divided[i] = 0;
          continue;
        }
        else if((res == 0) && (gv.ir.meas_ca.wt_sensor_info[i].state == 0))
        {
          wad_err_cnt[i] = 0;
          gv.ir.meas_ad.wad_divided[i] = wad_filter(&filter_buf_stru[i], (gv.ir.meas_ca.wt_sensor_info[i].srccode >> 6));
        }
        else
        {
          /* TODO: gv.ir.meas_ad.wad_divided[i] 暂且维持原值,等更合理的逻辑出来后再处理 */
          if(wad_err_cnt[i] < SENSOR_WAD_ERRCNT_MAX){
            wad_err_cnt[i]++;
          }else{
            wad_err_cnt[i] = SENSOR_WAD_ERRCNT_MAX;
          }
        }

        vTaskDelay(1);
      }
      
      sensor_get_spdcode_state();
      vTaskDelay(1);
    }

    if(gv.hr.meas_pa.wad_preset_enable == AD_CODE_ACTUAL) /* 实测 */
    {
      gv.ir.meas_ad.wad_unified = gv.ir.meas_ad.wad_divided[0]+ gv.ir.meas_ad.wad_divided[1] + \
                                  gv.ir.meas_ad.wad_divided[2]+ gv.ir.meas_ad.wad_divided[3];

      gv.ir.meas_ad.sad_hz = gv.ir.meas_ca.spd_sensor_info.srccode;
      gv.ir.meas_ad.sad_100hz = gv.ir.meas_ca.spd_sensor_info.srccode / 10;
    }
    else
    {
      gv.ir.meas_ad.wad_unified = gv.hr.meas_pa.wad_preset_value;
      gv.ir.meas_ad.sad_hz = gv.hr.meas_pa.sad_preset_value;
      gv.ir.meas_ad.sad_100hz = gv.hr.meas_pa.sad_preset_value / 10;
    }
  }
}
